package behavior;


import lejos.nxt.Button;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.robotics.navigation.TachoPilot;
/**
 * An object of this class is made to detect whether the robot is on a slope and if so,
 *  whether the slope is rising or not. This is detected with 2 touchsensors (one in front and
 *  one at the back of the robot). 
 * @author Simon & Maarten
 *
 */
public class SlopeDetector {

	private TachoPilot pilot;
	private TouchSensor touch = new TouchSensor(SensorPort.S3);
	private TouchSensor touch2= new TouchSensor(SensorPort.S4);

	public SlopeDetector(TachoPilot pilot) {
		this.pilot = pilot;
	}
	/**
	 * This method analyses the panel, the robot is on. First the robot drives a bit forward to
	 * the start of the next panel. The value endPress detects whether the front touchsensor is
	 * pressed at the start of this new panel. 
	 * When this endpress is true, we know that during this last drive, the robot was descending
	 * and the next panel is not. So the panel is "down" or "wip". 
	 * When the frontpress is true, we know that the robot is rising, so he's on panel "wip" or
	 * panel "up". 
	 * When the backReleased is true, we know that the robot is descending. So the panel is "down".
	 * @param frontPress	whether or not the touchsensor at the front of the robot
	 * 						was pressed before this analyse is called upon.
	 * @param backReleased	whether or not the touchsensor at the back of the robot
	 * 						was released from the ground before this analyse is called upon.
	 * @return the panel which the robot is on
	 */
	public int analyse(boolean frontPress, boolean backReleased, int previousPanel ) {
		
		boolean endPress = false;
		if(previousPanel==2)
			backReleased=false;
		if(previousPanel==4)
			frontPress=false;
		pilot.reset();
		float distance=0;		
		pilot.travel(20);
		sleep(1000);
		pilot.forward();
		while(distance<50 && !endPress){
			endPress = touch.isPressed();
			distance = pilot.getTravelDistance();
		}
		
		pilot.travel(-distance);
		
		if(frontPress && endPress)
			return 7; // wip
		else if(frontPress && !endPress)
			return 2; // up
		else if(backReleased && endPress)
			return 4; // down
		else
			return 1; // straight
		
	}

	public void Test() {
		int button = Button.waitForPress();
		if(button==1)
			pilot.forward();
		int i=0;
		while (true) {
			System.out.println(touch2.isPressed());
			
			
//			Motor motorR = (Motor) pilot.getRight();
//
//			motorL.flt();
//			motorR.flt();
//
//			sleep(500);
//
//			if (pilot.getTravelDistance() < 0) {
//				System.out.println("Up");
//			} else if (pilot.getTravelDistance() > 0) {
//				System.out.println("Down");
//			} else {
//				System.out.println("Flat");
//			}

		}
	}

	private void sleep(int milli) {
		try {
			Thread.sleep(milli);
		} catch (InterruptedException e) {

		}
	}

}
